多軸化感測需求興 MEMS元件整合勢不可當
為達成上述先進應用的需求，MEMS感測器的整合概念係利用各種感測器提供具互補性的資訊能力，再運用智慧技術整合這些資訊，進而達到最佳化系統性能的目的，實現令人震撼的新應用。而本文將以互補型濾波器、卡曼濾波器(Kalman Filter, KF)和擴展型卡曼濾波器(EKF)為例，論述在一個感測器整合解決方案內如何讓這些感測器協調工作，協助讀者了解如何實現MEMS感測器整合的概念。
解決測量偏差 MEMS多軸整合方案抬頭
在設計一個包含許多MEMS感測器的系統時，必須了解加速度計、陀螺儀、地磁計和壓力感測器的優缺點，這是很重要的第一步。而感測器整合解決方案可以解決六軸感測器模組的主要動作檢測性能問題，其由一個三軸加速度計和一個三軸陀螺儀，或一個三軸磁力感測器組成，進而達到六軸的效能。
因為陀螺儀在運轉一段時間後會發生測量偏差，所以一個由加速度計和陀螺儀組成的六軸慣性感測器模組可能會丟失絕對方位資訊，必須經過重新校準才能恢復精確的航向。如在有鐵材料的環境內，六軸感測器模組便非常容易發生資料損壞問題。有鑑於此，由加速度計、陀螺儀和磁力計組成的九軸感測器模組即可徹底解決單一感測器解決方案的測量值漂移問題，但是這些感測器很容易受到磁干擾，因此，整合感測器的演算法須要修正磁干擾的影響。
從上述需求可歸納出，感測器整合的目的是把每個感測器的測量資料作為輸入資料，然後應用數位濾波演算法對輸入資料進行相互補償，最終輸出精 的回應，以及快速的動態(傾斜/翻轉/偏移)測量結果。以下將以三種不同的濾波器來探討。
融合加速度計/陀螺儀 互補型濾波器終結角度失準
機器人平衡、數位相機圖像穩定和三維(3D)滑鼠等應用產品，內部均安裝一個加速度計和一個陀螺儀，當設備靜止時，加速度計可進行精確的傾斜角度測量；當設備旋轉或動作時，加速度計則跟不上快速的動作，而陀螺儀卻可輸出動態的角速率資料。但是，若只整合陀螺儀，雖然可計算出角位移或傾斜角度，然在長時間輸出資料後，因為陀螺儀零偏漂移，該角度將變得不夠精確。
為解決此一問題，互補型濾波器變應運而生，其為一個簡單融合加速度計和陀螺儀或磁力計，以獲得精確且反應快速的傾斜、翻轉、偏移等輸出訊號的方式。互補型濾波器由一個通用低通濾波器(Low Pass Filter, LPF)和一個高通濾波器(High Pass Filter, HPF)組成，低通濾波器用於過濾加速度計訊號；高通濾波器則用於過濾陀螺儀訊號。
互補型濾波器的原理較易理解，實現方式可參考卡曼濾波器。以一個實現機器人自動平衡用互補型濾波器為例(圖1)，機器人用雙軸或三軸加速度計來測量靜態傾斜角θa，再用單軸或雙軸陀螺儀測量動態傾斜角θg，然後互補型濾波器整合所有測量資料，得到最終的傾斜角θ。微控制器(MCU)利用該資訊和陀螺儀的角速率資訊控制馬達，使機器人保持平衡的姿態。
 |
| 圖1 自平衡機器人 |
而圖2是一個互補型濾波器的結構，其中有一個雙軸加速度計和一個單軸陀螺儀。可透過方程式(1)、(2)表達其結構：
 |
| 圖2 自平衡機器人的互補型濾波器 |
 |
‥‥‥‥‥‥‥‥‥‥方程式(1)
 |
‥‥‥‥‥方程式(2)
其中，aY和aZ是在加速度計原始資料中施加零g偏差和比例因數兩個校準參數後的[-1g+1g]範圍內的歸一化加速度值，θa是以度為單位的傾斜角。
而ωX為陀螺儀角速率原始資料，ωX0是以LSB為單位的零角速率電壓，S是以°/s/LSB為單位的靈敏度，ΔT則是採樣時間間隔，θg係以角位移。另一方面，互補型濾波器輸出最終傾斜角的運算過程則如方程式(3)：
 |
‥‥‥‥‥方程式(3)
β是介於0和1之間的常數。若設β為0.95，則將得出方程式(4)：
 |
‥‥‥‥‥‥‥‥方程式(4)
方程式(4)的第一部分像一個高通濾波器，准許機器人的動態動作通過陀螺儀。第二部分像一個低通濾波器，准許機器人的靜態或準靜態動作通過加速度計。
不受線性加速度/零偏漂移影響 互補型濾波器易導入MCU
值得一提的是，假設加速度計和陀螺儀的資料採樣速度是100Hz，則採樣間隔ΔT為0.01秒。所以互補型濾波器的時間常量則如方程式(5)所示：
 |
‥‥‥方程式(5)
若將加速度計和陀螺儀資料進行加權計算，互補型陀螺儀可視為一個簡易濾波器。當動作速度大於0.19秒間隔時，陀螺儀角位移θg的權數大於加速度計的權數，加速度計的雜訊被濾除。當動作速度小於0.19秒間隔時，加速度計傾斜度測量值θa的權數大於陀螺儀測量值θg，以降低陀螺儀的零偏漂移對垂直點的影響。因此，互補型濾波器估算的傾斜度既準確又響應快速，不易受到線性水準加速度和陀螺儀零偏漂移的影響。互補型濾波器比卡曼濾波器更容易在微控制器上實現。互補型濾波器還可以擴展，整合多軸加速度計和陀螺儀資料。
當陀螺儀的零角速率電壓或零偏漂移ωX0是一個常數且機器人靜止時，互補型濾波器輸出的傾斜角也有一個常數零偏漂移，可以用加速度計的傾斜角測量資料修正該偏差。如果這個偏差與時間和溫度有關，則互補型濾波器的傾斜角誤差將會隨時間變大。在這種情況下，當機器人上電且靜止時，須要取得ωX0值，以消除陀螺儀導通的漂移不穩定性。此外，當機器人在運行過程中處於靜止狀態時，須重新取得ωX0值，以消除運行中偏差不穩定性和短期角隨機漂移問題。
達成機器人自平衡 卡曼濾波器扮要角
達成機器人自平衡 卡曼濾波器扮要角
卡曼濾波器是一個數學演算法，發明者是卡曼博士。自1960年問世以來，卡曼濾波器被很多應用領域廣泛使用。人們最熟悉的卡曼濾波器應用是全球衛星定位系統(GPS)接收機，以及後來的GPS與慣性導航系統(INS)的整合產品。這種遞迴數位演算法用於整合GPS與加速度計和陀螺儀的測量資料，以提升導航系統的總體性能。
卡曼濾波器演算法可產生感測器真實測量值的估算值和相關計算值，具體的演算法程序是先預測一個數值，再估算被預測值的不確定性，而後計算被預測值和測量值的加權平均值，最終把最大權數給不確定性最低的數值。該演算法得出的估算值通常比原始測量值更接近真實值，因為與未經加權平均值計算的數值相比，加權平均值更精確估算測量值的不確定性。
與互補型濾波器相比，卡曼濾波器要求設計人員具有紮實的數學背景知識，包括隨機訊號處理、矩陣理論和控制理論。本部分介紹基本型離散卡曼濾波器演算法，以機器人自平衡為例，來論述如何實現此種濾波器。
卡曼濾波器根據線性差分方程式求出一個由離散時間控制的過程的狀態x值，如方程式(6)：
 |
‥‥‥‥‥方程式(6)
其中，A是在沒有策動點函數或過程雜訊情況下，使前一個時間步k-1的狀態與當前的時間步k的狀態相關聯的n×n矩陣。B是使可選控制輸入u與狀態x相關聯的n×1矩陣；H是使狀態與測量值zk相關聯的n×m矩陣；wk是過程雜訊(隨機變數)；vk是測量雜訊(隨機變數)。假設w和v是相互獨立的正常概率分布的高斯白色雜訊，則p(w)～N(0,Q)，p(v)～N(0,R)，Q代表過程雜訊共變異數矩陣，R為測量雜訊共變異數矩陣。
圖3則是基於線性動態系統的卡曼濾波器演算法，即矩陣A、B和H是常量。為簡便計算，假設過程和測量的共變異數Q和R分別為常數。然後，用每個時間步的卡曼增益Kk的計算值，對測量值進行連續的加權計算，使誤差共變異數矩陣Pk保持最小化，如此一來，被更新的狀態將會準確且回應快速。
 |
| 圖3 卡曼濾波器遞回演算法 |
回過頭來關注機器人平衡示例，針對此種應用嘗試實現卡曼濾波器演算法，以整合加速度計和陀螺儀的資料。第一步是建立狀態和測量方程式(6)。顯而易見的，最終傾斜角θ是為動態修正陀螺儀零偏漂移而準備估算的狀態，也是被關注的另一個狀態。此外，若想用加速度計測量值來限制最終傾斜角，讓陀螺儀不會因隨機漂移而隨著時間產生漂移事件。須設b=ωX0，並假定陀螺儀零偏漂移是常數且含有白色雜訊。首先，設陀螺儀測量值ωX為輸入uk，則方程式(2)可改寫為方程式(7)：
 |
‥‥‥‥‥‥方程式(7)
其中ΔT是採樣時隔。例如，當陀螺儀資料採樣速率是100Hz時，則ΔT=0.01秒。另根據方程式(1)，其測量值亦可寫作方程式(8)：
 |
‥‥‥‥‥方程式(8)
而合併方程式(7)、(8)後，更可推得方程式(9)：
 |
‥‥‥方程式(9)
透過方程式(9)不難看出矩陣A、B和H是常數，因此系統是線性的。
第二步則是從陀螺儀上的離線實驗求出過程的共變異數矩陣Q；從加速度計上的離線實驗求出測量的共變異數矩陣R，從而得出方程式(10)：
 |
‥方程式(10)
緊接著，第三步是用初始值x0和P0在微控制器上實現卡曼濾波器演算法，如圖3所示。最後一步則進入卡曼濾波器的性能測試，因為陀螺儀零偏漂移是在回路內最佳化估算的，卡曼濾波器輸出的機器人傾斜角一向很準確，進一步將卡曼濾波器的參數Q和R經過最終優化調整後，傾斜角將不會再發生任何漂移。
使估算值保持線性 擴展式卡曼濾波器益處多
可估算過程，或測量與過程關係呈非線性的應用，是卡曼濾波器最成功且最引人關注的應用案例，而進一步使平均數和共變異數，達成線性化的卡曼濾波器，就稱做擴展型卡曼濾波器。在類似於泰勒級數的函數內，工程師可使用測量函數的偏導數來計算估算值，使濾波器沿著當前的估算值保持線性，即便過程是非線性關係亦可獲得同樣的效果。
而要了解用於整合加速度計、陀螺儀和磁力計數據的四元數擴展型卡曼濾波器設計方式，必須先有載體坐標系和局部水準參照系之間旋轉矩陣，以及四元數與歐拉角之間旋轉矩陣的數學背景知識。
舉例來說，當把一個三軸加速度計、三軸陀螺儀和三軸磁力計作為一個九軸感測器模組，並安裝到一個捷聯式導航系統時，因為沒有歐拉角的特殊問題，所以四元數法常被用於基於陀螺儀測量資料的旋轉矩陣計算。如擴展式卡曼濾波器整合MEMS感測器資料，然後更新四元數和陀螺儀漂移，提供精確的傾斜、翻轉和偏移姿態資訊，這就是所謂的姿態航向參考系統(AHRS)。
基於此一發展，意法半導體(ST)已研製出九軸MEMS感測器演示板，這塊應用演示板被命名為iNemo(iNErtial MOdule V2)。如圖4所示，意法半導體在AHRS用STM32微控制器中，實現基於四元數的擴展式卡曼濾波器，當電路板在3D空間旋轉時，有魚圖案的正方體將跟隨電路板動作，並輸出即時、精確、動態的數值，如傾斜、翻轉、偏移及四元數值等。以下簡要介紹擴展式卡曼濾波器在iNemo V2電路板上的設計過程。
 |
| 圖4 在iNemo V2平台上的AHRS示例 |
基本上，設計擴展式卡曼濾波器須按下列步驟：首先用四元數法定義狀態和測量值；接著建立系統動態模型及測量值與狀態之間的關係；再把系統模型變成線性和離散，並計算雅可比矩陣，在每個時間步k計算矩陣A、B和H，以得出歸一化四元數值；最後便可計算傾斜、翻轉歐拉角。
效能/功耗兩相衝突 感測器整合演算法須取捨
當把多個MEMS感測器整合在一個系統時，須要在系統內實現感測器整合演算法，才能充分利用每個感測器的功能，以提高總體系統性能。對於卡曼濾波器和擴展式卡曼濾波器，雖然系統模型和感測器漂移模型的設計不同，但是基本的遞迴演算法保持不變。而卡曼濾波器和擴展式卡曼濾波器亦被視為感測器整合方案的核心技術。
另一方面，若從性能角度來看，擴展式卡曼濾波器將是最佳的解決方案。但是，該技術給主要處理器帶來巨大的計算負荷，可能會消耗大量的電力，以智慧型手機的應用來說，便是一個典型範例。因此，在選擇感測器整合演算法的決策中，性能與功耗的平衡始終是關鍵性的要素。
